#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
        ros::init(argc, argv, "my_tf_lis"); 
        ros::NodeHandle node;

        ros::service::waitForService("spawn");
        ros::ServiceClient  add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
        turtlesim::Spawn srv;
        add_turtle.call(srv);

        //turtle  vel publisher
        ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

        tf::TransformListener lis;

        ros::Rate rate(10.0);

        while (node.ok())
        {
                /* code */
                tf::StampedTransform transform;
                try
                {
                        lis.waitForTransform("/turtle2","turtle1", ros::Time(0), ros::Duration(3.0));
                        lis.lookupTransform("/turtle2","/turtle1",ros::Time(0), transform);
                }
                catch(tf::TransformException &ex)
                {
                        ROS_ERROR("%s",ex.what());
                        ros::Duration(1.0).sleep();
                        continue;
                }

                geometry_msgs::Twist vel_msg;
                vel_msg.angular.z = 2.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
                vel_msg.linear.x = 1* sqrt(pow(transform.getOrigin().x(),2) +pow(transform.getOrigin().y(),2)) ;
                turtle_vel.publish(vel_msg);

                rate.sleep();
        }
        return 0;
}